package Tests
{
	import Box2D.Collision.Shapes.b2CircleShape;
	import Box2D.Collision.Shapes.b2EdgeShape;
	import Box2D.Collision.Shapes.b2PolygonShape;
	import Box2D.Collision.Shapes.b2Shape;
	import Box2D.Common.Math.b2Vec2;
	import Box2D.Common.Math.b2Vec2Array;
	import Box2D.Dynamics.Joints.b2DistanceJointDef;
	import Box2D.Dynamics.Joints.b2RevoluteJoint;
	import Box2D.Dynamics.Joints.b2RevoluteJointDef;
	import Box2D.Dynamics.b2Body;
	import Box2D.Dynamics.b2BodyDef;
	import Box2D.Dynamics.b2BodyType;
	import Box2D.Dynamics.b2FixtureDef;
	
	import Framework.Test;
	
	public class TheoJansen extends Test
	{
		private var m_offset:b2Vec2 = new b2Vec2;
		private var m_chassis:b2Body;
		private var m_wheel:b2Body;
		private var m_motorJoint:b2RevoluteJoint;
		private var m_motorOn:Boolean;
		private var m_motorSpeed:Number

		public function CreateLeg(s:Number,wheelAnchor:b2Vec2):void
		{
			var p1:b2Vec2 = new b2Vec2(5.4 * s, -6.1);
			var p2:b2Vec2 = new b2Vec2(7.2 * s, -1.2);
			var p3:b2Vec2 = new b2Vec2(4.3 * s, -1.9);
			var p4:b2Vec2 = new b2Vec2(3.1 * s, 0.8);
			var p5:b2Vec2 = new b2Vec2(6.0 * s, 1.5);
			var p6:b2Vec2 = new b2Vec2(2.5 * s, 3.7);
			
			var fd1:b2FixtureDef = new b2FixtureDef;
			var fd2:b2FixtureDef = new b2FixtureDef;
			fd1.filter.groupIndex = -1;
			fd2.filter.groupIndex = -1;
			fd1.density = 1.0;
			fd2.density = 1.0;
			
			var poly1:b2PolygonShape = new b2PolygonShape;
			var poly2:b2PolygonShape = new b2PolygonShape;
			
			var vertices:b2Vec2Array = new b2Vec2Array(3);
			if (s > 0.0)
			{
				vertices.setAt(0,p1);
				vertices.setAt(1,p2);
				vertices.setAt(2,p3);
				poly1.Set(vertices, 3);
				
				vertices.getAt(0).SetZero();
				vertices.setAt(1,p5.Subtract(p4));
				vertices.setAt(2,p6.Subtract(p4));
				poly2.Set(vertices, 3);
			}
			else
			{
				vertices.setAt(0,p1);
				vertices.setAt(1,p3);
				vertices.setAt(2,p2);
				poly1.Set(vertices, 3);
				
				vertices.getAt(0).SetZero();
				vertices.setAt(1,p6.Subtract(p4));
				vertices.setAt(2,p3.Subtract(p4));
				poly2.Set(vertices, 3);
			}
			
			fd1.shape = poly1;
			fd2.shape = poly2;
			
			var bd1:b2BodyDef = new b2BodyDef;
			var bd2:b2BodyDef = new b2BodyDef;
			bd1.type = b2BodyType.b2_dynamicBody;
			bd2.type = b2BodyType.b2_dynamicBody;
			bd1.position = m_offset;
			bd2.position = p4.Add(m_offset);
			
			bd1.angularDamping = 10.0;
			bd2.angularDamping = 10.0;
			
			var body1:b2Body = m_world.CreateBody(bd1);
			var body2:b2Body = m_world.CreateBody(bd2);
			
			body1.CreateFixture(fd1);
			body2.CreateFixture(fd2);
			
			var djd:b2DistanceJointDef = new b2DistanceJointDef;
			
			// Using a soft distance constraint can reduce some jitter.
			// It also makes the structure seem a bit more fluid by
			// acting like a suspension system.
			djd.dampingRatio = 0.5;
			djd.frequencyHz = 10.0;
			
			djd.Initialize(body1, body2, p2.Add(m_offset), p5.Add(m_offset));
			m_world.CreateJoint(djd);
			
			djd.Initialize(body1, body2, p3.Add(m_offset), p4.Add(m_offset));
			m_world.CreateJoint(djd);
			
			djd.Initialize(body1, m_wheel, p3.Add(m_offset), wheelAnchor.Add(m_offset));
			m_world.CreateJoint(djd);
			
			djd.Initialize(body2, m_wheel, p6.Add(m_offset), wheelAnchor.Add(m_offset));
			m_world.CreateJoint(djd);
			
			var rjd:b2RevoluteJointDef = new b2RevoluteJointDef;
			
			rjd.Initialize(body2, m_chassis, p4.Add(m_offset));
			m_world.CreateJoint(rjd);
		}

		public function TheoJansen()
		{
			super();
			m_offset.Set(0,8);
			m_motorSpeed = 2;
			m_motorOn = true;
			
			createGround();
			createBalls();
			createChassis();
		}
		private function createGround():void{
			var shape:b2EdgeShape = new b2EdgeShape;;
			shape.Set(new b2Vec2(-50.0, 0.0), new b2Vec2(50.0, 0.0));
			m_groundBody.CreateFixtureByShape(shape, 0.0);
			
			shape.Set(new b2Vec2(-50.0, 0.0), new b2Vec2(-50.0, 10.0));
			m_groundBody.CreateFixtureByShape(shape, 0.0);
			
			shape.Set(new b2Vec2(50.0, 0.0), new b2Vec2(50.0, 10.0));
			m_groundBody.CreateFixtureByShape(shape, 0.0);						
		}
		private function createBalls():void{
			for (var i:int = 0; i < 40; ++i)
			{
				var shape:b2CircleShape = new b2CircleShape;
				shape.radius = 0.25;
				
				var bd:b2BodyDef = new b2BodyDef;
				bd.type = b2BodyType.b2_dynamicBody;
				bd.position.Set(-40.0 + 2.0 * i, 0.5);
				
				var body:b2Body = m_world.CreateBody(bd);
				body.CreateFixtureByShape(shape, 1.0);
			}			
		}
		private function createChassis():void{
			var pivot:b2Vec2 = new b2Vec2(0,.8);
			var sd:b2FixtureDef = new b2FixtureDef;
			var bd:b2BodyDef = new b2BodyDef;			
			{
				var shape:b2PolygonShape = new b2PolygonShape;
				shape.SetAsBox(2.5, 1.0);
				
				sd.density = 1.0;
				sd.shape = shape;
				sd.filter.groupIndex = -1;
				bd.type = b2BodyType.b2_dynamicBody;
				bd.position = pivot.Add(m_offset);
				m_chassis = m_world.CreateBody(bd);
				m_chassis.CreateFixture(sd);
			}
			
			{
				var shape2:b2CircleShape = new b2CircleShape;
				shape2.radius = 1.6;
				
				sd.density = 1.0;
				sd.shape = shape2;
				sd.filter.groupIndex = -1;
				bd.type = b2BodyType.b2_dynamicBody;
				bd.position = pivot.Add(m_offset);
				m_wheel = m_world.CreateBody(bd);
				m_wheel.CreateFixture(sd);
			}
			
			{
				var jd:b2RevoluteJointDef = new b2RevoluteJointDef;
				jd.Initialize(m_wheel, m_chassis, pivot.Add(m_offset));
				jd.collideConnected = false;
				jd.motorSpeed = m_motorSpeed;
				jd.maxMotorTorque = 400.0;
				jd.enableMotor = m_motorOn;
				m_motorJoint = m_world.CreateJoint(jd) as b2RevoluteJoint;
			}
			
			var wheelAnchor:b2Vec2;
			
			wheelAnchor = pivot.Add(new  b2Vec2(0.0, -0.8));
			
			CreateLeg(-1.0, wheelAnchor);
			CreateLeg(1.0, wheelAnchor);
			
			m_wheel.SetTransform(m_wheel.GetPosition(), 120.0 * Math.PI / 180.0);
			CreateLeg(-1.0, wheelAnchor);
			CreateLeg(1.0, wheelAnchor);
			
			m_wheel.SetTransform(m_wheel.GetPosition(), -120.0 * Math.PI / 180.0);
			CreateLeg(-1.0, wheelAnchor);
			CreateLeg(1.0, wheelAnchor);			
		}
		public override function Key(code:String):void{
			switch(code){
				case 'a':
					m_motorJoint.SetMotorSpeed(-m_motorSpeed);
					break;
				case 's':
					m_motorJoint.SetMotorSpeed(0);
					break;
				case 'd':
					m_motorJoint.SetMotorSpeed(m_motorSpeed);
					break;
				case 'm':
					m_motorJoint.EnableMotor(!m_motorJoint.IsMotorEnabled());
					break;
			}
		}
		public override function get hotKeyNote():String{
			return '(a) left (s) break (d) right (m) toggle motor';
		}
	}
}